package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import java.util.TimerTask;

/**
 *
 * @author Wesley
 */
public class Controller extends TimerTask
{
    private Joystick driverJoy;
    private Joystick rotationJoy;
    private DriveTrain dt;
    private boolean isInitialized = false;

    Controller()
    {
        driverJoy = new Joystick(1);
        rotationJoy = new Joystick(2);
    }

    public void initialize()
    {
        System.out.println("DriveTrain::initialize");
        if(!isInitialized)
        {
            dt = new DriveTrain();
            isInitialized = true;
        }
    }

    public void run()
    {
        if(DriverStation.getInstance().isAutonomous())
        {
            driveAuto();
        }
        else
        {
            driveTele();
        }
    }

    private void driveAuto()
    {
        //nothing for now
    }

    private void driveTele()
    {
        double xVal = driverJoy.getX();
        double yVal = driverJoy.getY();
        double rot = rotationJoy.getTwist();

        if(!isSnapButtonPressed())
        {
            int snapAngle = getSnapAngle();

            if(snapAngle != -1)
            {
                dt.driveTo(snapAngle);
            }
        }
        else
        {
            dt.driveTo(xVal, yVal, rot);
        }
    }

    private boolean isSnapButtonPressed()
    {
        if(driverJoy.getRawButton(Utils.SNAP_0_BUTTON_NUM) ||
           driverJoy.getRawButton(Utils.SNAP_90_BUTTON_NUM) ||
           driverJoy.getRawButton(Utils.SNAP_180_BUTTON_NUM) ||
           driverJoy.getRawButton(Utils.SNAP_270_BUTTON_NUM))
           {
                return true;
           }

        return false;
    }

    /**
     * Determines if a snap button is pressed and if it is, return the angle
     * of the button that is pressed
     * @return angle
     */
    private int getSnapAngle()
    {
        if(driverJoy.getRawButton(Utils.SNAP_0_BUTTON_NUM))
        {
            return 0;
        }
        else if(driverJoy.getRawButton(Utils.SNAP_90_BUTTON_NUM))
        {
            return 90;
        }
        else if(driverJoy.getRawButton(Utils.SNAP_180_BUTTON_NUM))
        {
            return 180;
        }
        else if(driverJoy.getRawButton(Utils.SNAP_270_BUTTON_NUM))
        {
            return 270;
        }
        else
        {
            return -1;
        }
    }
    
    public void zeroWheels()
    {
        dt.zeroWheels();
    }

    public boolean isInitialized()
    {
        return isInitialized;
    }

}
